#include "sleepstate.h"
#include "fsm.h"
#include <iostream>
#include "masters/vehiclecontroller.h"
using namespace std;
using namespace vehicle;

SleepState::SleepState()
{

}



void SleepState::Update(FSM *fsm)
{
    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();

    ushort sn=0;
    int ret=dhdGetSerialNumber(&sn,master_device);
    if(ret!=0)
    {
        master_device=-1;
        fsm->vehicle_controller_->SetMasterDeviceIndex(-1);
    }
    if(master_device<0)
        return;
    if(fsm->vehicle_controller_->GetRunningStatus()==main_controller::kRCWorking)
        fsm->SetState(BaseState::kPrepareState);

}

void SleepState::OnEntry(FSM *fsm)
{
    cout<<"entry sleep state"<<endl;
    //drdStop(true,fsm->vehicle_controller_->GetMasterDeviceIndex());
    //drdClose(fsm->vehicle_controller_->GetMasterDeviceIndex());

    char master_device=fsm->vehicle_controller_->GetMasterDeviceIndex();
    if(master_device<0)
        return;
    dhdSetForceAndGripperForce(0,0,0,0,master_device);

}

void SleepState::OnExit(FSM *fsm)
{
    cout<<"exit sleep state"<<endl;

}



